import Joint from "./joint";
import CONSTANT from "../../constant";
import Vec3 from "../../common/vec3";
import Method from "../../common/method";

export default class UniversalJoint extends Joint{
    constructor(config) {
        super(config,CONSTANT.JOINT_TYPE_UNIVERSAL);
        Method.copyElements(config.localAxis1.elements,this.localBasis1,0,0,3);
        Method.copyElements(config.localAxis2.elements,this.localBasis2,0,0,3);
        this.buildLocalBasesFromX1Z2();
        this.angle=new Float64Array(3);
        this.singular=[false,false,false];
        this._sd1 = config.springDamper1.clone();
        this._sd2 = config.springDamper2.clone();
        this._lm1 = config.limitMotor1.clone();
        this._lm2 = config.limitMotor2.clone();
        this.linearError=new Float64Array(3);
        this.axisX=new Float64Array(3);
        this.axisY=new Float64Array(3);
        this.axisZ=new Float64Array(3);
    }

    getInfo(info,timeStep,isPositionPart) {
        let erp = this.getErp(timeStep,isPositionPart);
        let linearRhsX = this.linearError[0] * erp,linearRhsY = this.linearError[1] * erp,linearRhsZ = this.linearError[2] * erp;
        let angRhsY = this.angle[1] * erp;
        let ra1=this.relativeAnchor1, ra2=this.relativeAnchor2;
        let ax=this.axisX,ay=this.axisY,az=this.axisZ;
        let c100 = 0, c101 = ra1[2], c102 = -ra1[1];
        let c110 = -ra1[2], c111 = 0, c112 = ra1[0];
        let c120 = ra1[1], c121 = -ra1[0], c122 = 0;
        let c200 = 0, c201 = ra2[2], c202 = -ra2[1];
        let c210 = -ra2[2], c211 = 0, c212 = ra2[0];
        let c220 = ra2[1], c221 = -ra2[0], c222 = 0;
        let motorMassX = this.computeEffectiveInertiaMoment(ax[0],ax[1],ax[2]);
        let motorMassZ = this.computeEffectiveInertiaMoment(az[0],az[1],az[2]);
        let impulse = this._impulses[0];
        let row = info.rows[info.numRows++];
        this._resetRow(row,impulse);
        row.rhs = linearRhsX;
        row.cfm = 0;
        row.minImpulse = -1e65536;
        row.maxImpulse = 1e65536;
        Method.setJacobianElements(row.jacobian.elements,1,0,0,1,0,0,c100,c101,c102,c200,c201,c202);
        let impulse1 = this._impulses[1];
        let row1 = info.rows[info.numRows++];
        this._resetRow(row1,impulse1);
        row1.rhs = linearRhsY;
        row1.cfm = 0;
        row1.minImpulse = -1e65536;
        row1.maxImpulse = 1e65536;
        Method.setJacobianElements(row1.jacobian.elements,0,1,0,0,1,0,c110,c111,c112,c210,c211,c212);
        let impulse2 = this._impulses[2];
        let row2 = info.rows[info.numRows++];
        this._resetRow(row2,impulse2);
        row2.rhs = linearRhsZ;
        row2.cfm = 0;
        row2.minImpulse = -1e65536;
        row2.maxImpulse = 1e65536;
        Method.setJacobianElements(row2.jacobian.elements,0,0,1,0,0,1,c120,c121,c122,c220,c221,c222);
        if(!this.singular[0] && (this._sd1.frequency <= 0 || !isPositionPart)) {
            let impulse = this._impulses[3];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowAngular(row,this.angle[0],this._lm1,motorMassX,this._sd1,timeStep,isPositionPart);
            let j = row.jacobian.elements;
            j[6] = ax[0];j[7] = ax[1];j[8] = ax[2];
            j[9] = ax[0];j[10] = ax[1];j[11] = ax[2];
        }
        if(!this.singular[1]) {
            let impulse = this._impulses[4];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            row.rhs = angRhsY;
            row.cfm = 0;
            row.minImpulse = -1e65536;
            row.maxImpulse = 1e65536;
            let j = row.jacobian.elements;
            j[6] = ay[0];j[7] = ay[1];j[8] = ay[2];
            j[9] = ay[0];j[10] = ay[1];j[11] = ay[2];
        }
        if(!this.singular[2] && (this._sd2.frequency <= 0 || !isPositionPart)) {
            let impulse = this._impulses[5];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowAngular(row,this.angle[2],this._lm2,motorMassZ,this._sd2,timeStep,isPositionPart);
            let j = row.jacobian.elements;
            j[6] = az[0];j[7] = az[1];j[8] = az[2];
            j[9] = az[0];j[10] = az[1];j[11] = az[2];
        }
    }
    _syncAnchors() {
        super._syncAnchors();
        let ax=this.axisX,ay=this.axisY,az=this.axisZ;
        let bs1=this.basis1,bs2=this.basis2;
        let tm=this._tm,tv=this._tv;
        Method.makeBasis(bs1[0],bs1[1],bs1[2],bs2[6],bs2[7],bs2[8],tm);
        Method.copyElements(tm,ax,0,0,3);
        Method.copyElements(tm,ay,3,0,3);
        Method.copyElements(tm,az,6,0,3);
        this.singular[0] = ax[0] * ax[0] + ax[1] * ax[1] + ax[2] * ax[2] === 0;
        this.singular[1] = ay[0] * ay[0] + ay[1] * ay[1] + ay[2] * ay[2] === 0;
        this.singular[2] = az[0] * az[0] + az[1] * az[1] + az[2] * az[2] === 0;
        Method.multiplyBasis(bs2,bs1,tm);
        Method.mat3ToVec3(tm,tv);
        Method.copyElements(tv,this.angle,0,0,3);
        Method.subArray(this.anchor2,this.anchor1,this.linearError,0,0,0,3);
    }
    _getVelocitySolverInfo(timeStep,info) {
        super._getVelocitySolverInfo(timeStep,info);
        this.getInfo(info,timeStep,false);
    }
    _getPositionSolverInfo(info) {
        super._getPositionSolverInfo(info);
        this.getInfo(info,null,true);
    }
    getAxis1() {
        return new Vec3(this.basis1[0],this.basis1[1],this.basis1[2]);
    }
    getAxis2() {
        return new Vec3(this.basis2[6],this.basis2[7],this.basis2[8]);
    }
    getAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.basis1[0];es[1] = this.basis1[1];es[2] = this.basis1[2];
    }
    getAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.basis2[6];es[1] = this.basis2[7];es[2] = this.basis2[8];
    }
    getLocalAxis1() {
        return new Vec3(this.localBasis1[0],this.localBasis1[1],this.localBasis1[2]);
    }
    getLocalAxis2() {
        return new Vec3(this.localBasis2[6],this.localBasis2[7],this.localBasis2[8]);
    }
    getLocalAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis1[0];es[1] = this.localBasis1[1];es[2] = this.localBasis1[2];
    }
    getLocalAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis2[6];es[1] = this.localBasis2[7];es[2] = this.localBasis2[8];
    }
    getSpringDamper1() {
        return this._sd1;
    }
    getSpringDamper2() {
        return this._sd2;
    }
    getLimitMotor1() {
        return this._lm1;
    }
    getLimitMotor2() {
        return this._lm2;
    }
    getAngle1() {
        return this.angle[0];
    }
    getAngle2() {
        return this.angle[2];
    }
}